package com.gitee.wsl.mathematics.geometry.d2.curve.ext

import com.gitee.wsl.ext.number.lerp
import kotlin.math.floor
import kotlin.math.sqrt


fun Double.fposmod(b: Double): Double {
    val value = this.rem(b)
    if ((value < 0 && b > 0) || (value > 0 && b < 0)) {
        return value + b;
    }
    return value
}

fun snapped(value: Int, step: Int) = if (step != 0) {
    (floor(value.toDouble() / step.toDouble() + 0.5) * step).toInt()
} else {
    value
}

fun snapped(value: Double, step: Double) = if (step != 0.0) {
    floor(value / step + 0.5) * step
} else {
    value
}

/* quadratic bezier */
fun bezier(x0: Double, c0: Double, x1: Double, t: Double): Double {
    val it = 1.0 - t
    val it2 = it * it
    val t2 = t * t
    return it2 * x0 + 2.0 * it * t * c0 + t2 * x1
}

fun derivative(x0: Double, c0: Double, x1: Double, t: Double): Double {
    val it = 1.0 - t
    return 2.0 * it * (c0 - x0) + 2.0 * t * (x1 - c0)
}


fun roots(p: List<Float>): List<Float> {
    //https://github.com/Pomax/bezierjs/blob/gh-pages/lib/utils.js
    if (p.size == 3) {
        val a = p[0]
        val b = p[1]
        val c = p[2]
        val d = a - 2 * b + c
        if (d != 0.0f) {
            val m1 = -sqrt(b * b - a * c)
            val m2 = -a + b
            val v1 = -(m1 + m2) / d
            val v2 = -(-m1 + m2) / d
            return listOf(v1, v2)
        } else if (b != c && d == 0.0f) {
            return listOf((2 * b * c) / (2 * (b - c)))
        }
        return emptyList()
    } else if (p.size == 2) {
        val a = p[0]
        val b = p[1]
        return if (a != b) {
            listOf(a / (a - b))
        } else {
            emptyList()
        }
    }
    return emptyList()

}


fun derivative2(p0: Double, p1: Double, p2: Double, p3: Double, t: Double): Double {
    return (p2 - p1 * 2.0 + p0) * (6 * (1.0 - t)) + (p3 - p2 * 2.0 + p1) * (6.0 * t)
}


/* cubic bezier */
fun bezier(x0: Double, c0: Double, c1: Double, x1: Double, t: Double): Double {
    val it = 1.0 - t
    val it2 = it * it
    val it3 = it2 * it
    val t2 = t * t
    val t3 = t2 * t

    return it3 * x0 + 3.0 * it2 * t * c0 + 3.0 * it * t2 * c1 + t3 * x1
}


// linear type bezier

/* quadratic bezier */
/*
fun <T: LinearType<T>> bezier(x0: T, c0: T, x1: T, t: Double): T {
    val it = 1.0 - t
    val it2 = it * it
    val t2 = t * t
    return x0 * it2 + c0 * (2.0 * it * t) + x1 * t2
}

fun <T : LinearType<T>> bezier(x0: T, c0: T, c1: T, x1: T, t: Double): T {
    val it = 1.0 - t
    val it2 = it * it
    val it3 = it2 * it
    val t2 = t * t
    val t3 = t2 * t
    return x0 * (it3) + c0 * (3 * it2 * t) + c1 * (3 * it * t2) + x1 * (t3)
}
*/



fun bezierInterpolate(start: Double, control1: Double, control2: Double, end: Double, t: Double): Double {
    /* Formula from Wikipedia article on Bezier curves. */
    val omt: Double = 1.0 - t
    val omt2 = omt * omt
    val omt3 = omt2 * omt
    val t2: Double = t * t
    val t3: Double = t2 * t

    return start * omt3 + control1 * omt2 * t * 3.0 + control2 * omt * t2 * 3.0 + end * t3
}

fun cubicInterpolate(
    from: Double,
    to: Double,
    pre: Double,
    post: Double,
    weight: Double
) = 0.5f *
        ((from * 2.0f) +
                (-pre + to) * weight +
                (2.0f * pre - 5.0f * from + 4.0f * to - post) * (weight * weight) +
                (-pre + 3.0f * from - 3.0f * to + post) * (weight * weight * weight))


fun cubicInterpolateInTime(
    from: Double,
    to: Double,
    pre: Double,
    post: Double,
    weight: Double,
    toT: Double,
    preT: Double,
    postT: Double
): Double {
    val t = lerp(0.0, toT, weight)
    val a1 = lerp(pre, from, if (preT == 0.0) 0.0 else (t - preT) / -preT)
    val a2 = lerp(from, to, if (toT == 0.0) 0.5 else t / toT)
    val a3 = lerp(to, post, if (postT - toT == 0.0) 1.0 else (t - toT) / (postT - toT))
    val b1 = lerp(a1, a2, if (toT - preT == 0.0) 0.0 else (t - preT) / (toT - preT))
    val b2 = lerp(a2, a3, if (postT == 0.0) 1.0 else t / postT)
    return lerp(b1, b2, if (toT == 0.0) 0.5 else t / toT)
}
